package com.google.android.apps.skipbot;

import android.os.AsyncTask;

public class GenericIKGaitSequence extends SkipperMovementSequence {
  private final IKSequenceProvider sequenceConfig;
  /**
   * The direction of the gait sequence. This should never be accessed directly
   * other than in the constructor as it may be modified in a different thread
   * than it is read.
   */
  private double angle;

  public GenericIKGaitSequence(SkipperRobotConfigInterface skipperRobotConfig,
      SkipperRobotController robotController, IKSequenceProvider sequenceConfig, double angle) {
    super(skipperRobotConfig, robotController);
    this.sequenceConfig = sequenceConfig;
    this.angle = angle;
  }

  /**
   * Gets the current direction of the gait sequence in radians.
   */
  public synchronized double getAngle() {
    return angle;
  }

  /**
   * Sets the angle in which the gait sequence will move. This value can be
   * changed while the sequence is executing.
   * 
   * @param angle direction angle in radians measured clockwise looking down on
   *        the robot; 0 is forward
   */
  public synchronized void setAngle(double angle) {
    this.angle = angle;
  }

  /**
   * Gets the current sequence config.
   */
  public IKSequenceProvider getSequenceConfig() {
    return sequenceConfig;
  }

  @Override
  public void run(AsyncTask<RobotMovementSequence, Integer, Boolean> asyncTask) {
    try {
      final int numLegs = skipperRobotConfig.getNumLegs();

      final int numberOfFrames = sequenceConfig.getNumberOfFrames();
      final int numberOfCycles =
          sequenceConfig.getNumberOfCycles() == -1 ? Integer.MAX_VALUE : sequenceConfig
              .getNumberOfCycles();
      final int framePause = sequenceConfig.getFramePause();
      final double[][] stridePosition = sequenceConfig.getStridePosition();
      final int[] initialLegStridePosition = sequenceConfig.getInitialLegStridePosition();
      final double[][] legBasePosition = sequenceConfig.getLegBasePosition();

      for (int i = 0; i < numberOfCycles; ++i) {
        for (int j = 0; j < numberOfFrames; ++j) {
          for (int k = 0; k < numLegs; ++k) {
            int currentStrideIndex = (j + initialLegStridePosition[k]) % numberOfFrames;

            double[] stride = stridePosition[currentStrideIndex];
            double[] basePosition = legBasePosition[k];

            final double currentAngle = getAngle();
            double[] position =
                {basePosition[0] + stride[0] * Math.sin(currentAngle),
                    basePosition[1] + stride[0] * Math.cos(currentAngle),
                    basePosition[2] + stride[1]};
            skipperRobotController.moveLeg(k, position);
          }
          // Pause to let the legs move.
          Thread.sleep(framePause);
        }
      }
    } catch (InterruptedException ignore) {
      // Stop and return.
      return;
    } finally {
      // TODO(bbrown): Move to a know good stopping state.
    }
  }
}
